#include "CFilt2.h"


CFilt2::CFilt2()
{
   wn = 1;
   ksi = 1;
}


CFilt2::CFilt2(double wn_,double ksi_)
{
   wn = wn_;
   ksi = ksi_;
}

CFilt2::~CFilt2()
{

}

void CFilt2::setParam(double wn_,double ksi_)
{
   wn = wn_;
   ksi = ksi_;
}

void CFilt2::init()
{
  int i;
  for(i=0;i<4;i++)
  {
   x[i] = 0;
  }
}

void CFilt2::simulate(double u,double dt)
{

    double xn[2];
  
    xn[0] = x[0]+ (x[3]+x[1])*dt/2 ;
    xn[1] = x[1] + (x[2] + wn*wn*(u-x[0])-2*ksi*wn*x[1])*dt/2;
 
    x[2] =   wn*wn*(u-x[0])-2*ksi*wn*x[1];
    x[3] = x[1];
    x[0] = xn[0];
    x[1] = xn[1];
    

}